/**
 * sched_opt.c
 * 协作式优先级调度器（优化版）
 *
 * 依赖 HAL_GetTick() 提供 ms 时基（STM32 HAL）
 * 请在工程中链接并在 main 初始化 HAL 时同时调用 SchedOpt_Init()
 */

#include "mid_scheduler.h"
#include <string.h>
#include <stdlib.h>

/* 如果在你的平台上中断函数名称不同，请替换这两个宏 */
#ifndef __disable_irq
extern void __disable_irq(void);
#endif
#ifndef __enable_irq
extern void __enable_irq(void);
#endif
#ifndef __WFI
/* fallback: 如果平台没有 __WFI，定义为空或使用平台等价 */
#define __WFI() do { __asm volatile ("wfi"); } while(0)
#endif

/* INTERNALS */
#define NULL_TID 0xFF

typedef struct {
    task_fn_t fn;
    uint8_t priority;
    task_state_t state;

    /* volatile 确保中断写入的可见性 */
    volatile evt_t event_flags;     /* 已合并（主线程）事件位 */
    volatile evt_t isr_event_flags; /* ISR 写入事件位 */
    volatile evt_t delayed_evt;     /* 延时事件（累加） */
    volatile uint32_t delay_until;  /* 延时到期 tick（0 表示无） */

    uint8_t next; /* 链表 next 指针 */
	uint8_t in_queue;	/* 防御性标志：是否已在就绪队列中（0/1） */
} s_task_t;

/* 静态表（固定内存，无堆分配） */
static s_task_t s_tasks[SCHED_MAX_TASKS];
static volatile uint8_t s_task_count = 0;

/* 每优先级就绪队列 head/tail（存储 tid） */
static uint8_t s_prio_head[SCHED_MAX_PRIORITY + 1];
static uint8_t s_prio_tail[SCHED_MAX_PRIORITY + 1];

/* ready bitmap：优先级队列非空标志 */
static uint32_t s_ready_bitmap = 0;

/* 默认错误处理（weak，用户可覆盖） */
__attribute__((weak)) void SchedOpt_ErrorHandler(const char *msg)
{
    /* 默认行为：空实现（调试时可覆盖此函数，例如把 msg 发送到串口或触发断言） */
    (void)msg;
}


/* 辅助：检查 tid 是否有效 */
static inline int valid_tid(int tid) {
    return (tid >= 0) && ((uint8_t)tid < s_task_count);
}

/* 系统 tick 获取（默认 HAL_GetTick） */
uint32_t SchedOpt_GetTick(void) {
    return HAL_GetTick();
}

/* ---------------- 内部不带中断保护的队列操作（调用者须持有 IRQ disabled） -------------- */
/* 从优先级队列中移除指定任务（若存在） */
/* 调用者须保证在合适上下文（若来自 ISR，应使用 SetEventFromISR 模式） */
static void dequeue_task(uint8_t tid) {
    if (!valid_tid(tid)) return;
    uint8_t prio = s_tasks[tid].priority;
    uint8_t prev = NULL_TID;
    uint8_t cur = s_prio_head[prio];
    uint8_t cnt = 0;
    while (cur != NULL_TID && cnt < s_task_count) {
        if (cur == tid) {
            uint8_t next = s_tasks[cur].next;
            if (prev == NULL_TID) {
                s_prio_head[prio] = next;
            } else {
                s_tasks[prev].next = next;
            }
            if (s_prio_tail[prio] == cur) s_prio_tail[prio] = prev;
            s_tasks[cur].next = NULL_TID;
			s_tasks[cur].in_queue = 0; /* 清除 in_queue 标志 */
            if (s_prio_head[prio] == NULL_TID) s_ready_bitmap &= ~(1U << prio);
			return;
        }
        prev = cur;
        cur = s_tasks[cur].next;
        ++cnt;
    }
		/* 如果遍历超过 s_task_count 仍未找到，说明队列可能损坏，触发错误处理 */
    if (cnt >= s_task_count) {
        SchedOpt_ErrorHandler("dequeue_task: list traversal exceeded s_task_count");
    }
}

/* 将任务入队（仅当 state == READY）；
   调用者在主上下文或合并时使用。 */
static void enqueue_ready(uint8_t tid) {
    if (!valid_tid(tid)) return;
    if (s_tasks[tid].state != TASK_STATE_READY){
			return; /* 严格检查状态 */
		}
		/* if already in queue, nothing to do */
    if (s_tasks[tid].in_queue) {
        return;
    }
	s_tasks[tid].in_queue = 1;
    uint8_t prio = s_tasks[tid].priority;
    s_tasks[tid].next = NULL_TID;
    if (s_prio_tail[prio] == NULL_TID) {
        s_prio_head[prio] = s_prio_tail[prio] = tid;
    } else {
        s_tasks[s_prio_tail[prio]].next = tid;
        s_prio_tail[prio] = tid;
    }
    s_ready_bitmap |= (1U << prio);
}

/* 从指定优先级队列 head 出队（返回 tid 或 -1） */
static int dequeue_ready_from_prio(int prio) {
    if (prio > SCHED_MAX_PRIORITY) return -1;
    uint8_t tid = s_prio_head[prio];
    if (tid == NULL_TID){
		return -1;
	}
    s_prio_head[prio] = s_tasks[tid].next;
    if (s_prio_head[prio] == NULL_TID) s_prio_tail[prio] = NULL_TID;
    s_tasks[tid].next = NULL_TID;
	s_tasks[tid].in_queue = 0; /* 标记为已出队 */
    if (s_prio_head[prio] == NULL_TID) s_ready_bitmap &= ~(1U << prio);
    return (int)tid;
}

/* 选最高优先级（0 为最高优先级） */
static int pick_highest_priority_index(void) {
    if (s_ready_bitmap == 0) return -1;
#if defined(__GNUC__) || defined(__clang__)
    return __builtin_ctz(s_ready_bitmap);
#else
    for (int i = 0; i <= SCHED_MAX_PRIORITY; ++i){
			if (s_ready_bitmap & (1U << i)) return i;
		}
    return -1;
#endif
}

/* 将 ISR 写入的事件合并到 event_flags（在临界区内） */
/* 这里我们在函数内部统一做临界区保护，因为它通常由主循环调用 */
static void merge_isr_events(void) {
    __disable_irq();
    for (uint8_t i = 0; i < s_task_count; ++i) {
    	if(s_tasks[i].state == TASK_STATE_SUSPENDED || s_tasks[i].state == TASK_STATE_DELETED){
    		continue;
    	}
        evt_t isr_ev = s_tasks[i].isr_event_flags;
        if (isr_ev) {
            s_tasks[i].event_flags |= isr_ev;
            s_tasks[i].isr_event_flags = 0;
            if (s_tasks[i].state != TASK_STATE_READY) {
                s_tasks[i].state = TASK_STATE_READY;
                enqueue_ready(i);
            }
        }
    }
    __enable_irq();
}

/* 检查延时到期（在主循环中调用） */
static void check_delayed_events(void) {
    uint32_t now = SchedOpt_GetTick();
    for (uint8_t i = 0; i < s_task_count; ++i) {
        s_task_t *t = &s_tasks[i];
        if(t->state == TASK_STATE_SUSPENDED || t->state == TASK_STATE_DELETED){
			continue;
		}
        uint32_t du = t->delay_until;
        if (du != 0) {
            if ((int32_t)(now - du) >= 0) {
            	__disable_irq();
                if (t->delayed_evt) {
                    t->event_flags |= t->delayed_evt;
                    t->delayed_evt = 0;
                }
                t->delay_until = 0;
                if(!t->in_queue){
                	t->state = TASK_STATE_READY;
                	enqueue_ready(i);
                }
                __enable_irq();
            }
        }
    }
}

/* ========== API ========== */

void SchedOpt_Init(void) {
    memset(s_tasks, 0, sizeof(s_tasks));
    s_task_count = 0;
    for (int i = 0; i <= SCHED_MAX_PRIORITY; ++i) {
        s_prio_head[i] = NULL_TID;
        s_prio_tail[i] = NULL_TID;
    }
    s_ready_bitmap = 0;
		/* 确保 in_queue 字段初始化（memset 已完成，但显式化便于审计） */
    for (int i = 0; i < SCHED_MAX_TASKS; ++i) {
        s_tasks[i].in_queue = 0;
        s_tasks[i].next = NULL_TID;
    }
}

/* 注册任务（建议在调度器启动前完成） */
tid_t SchedOpt_CreateTask(task_fn_t fn, uint8_t priority) {
    if (fn == NULL) return 0xFF;
    if (priority > SCHED_MAX_PRIORITY) return 0xFF;
	__disable_irq();
    if (s_task_count >= SCHED_MAX_TASKS){
			__enable_irq();
			return 0xFF;
	}
    uint8_t tid = s_task_count++;
		/* 在临界区内初始化任务，以避免并发读取到未初始化数据 */
    s_task_t *t = &s_tasks[tid];
    t->fn = fn;
    t->priority = priority;
    t->state = TASK_STATE_READY;
    t->event_flags = 0;
    t->isr_event_flags = 0;
    t->delayed_evt = 0;
    t->delay_until = 0;
    t->next = NULL_TID;
	t->in_queue = 0;	/* 初始化为未入队 */
	/* 将任务入队（在临界区内执行，保证原子性） */
    enqueue_ready(tid);
	__enable_irq();
    return tid;
}

/* 主线程设置事件并保证任务就绪（临界区保护） */
void SchedOpt_SetEvent(tid_t tid, evt_t evt) {
    if (!valid_tid(tid)) return;
    __disable_irq();
    s_tasks[tid].event_flags |= evt;
    if (s_tasks[tid].state != TASK_STATE_READY) {
        s_tasks[tid].state = TASK_STATE_READY;
        enqueue_ready(tid);
    }
    __enable_irq();
}

/* ISR-safe 设置事件：做原子 OR（若支持原子内建） */
void SchedOpt_SetEventFromISR(tid_t tid, evt_t evt) {
    if (!valid_tid(tid)) return;
#if defined(__GNUC__) || defined(__clang__)
    /* 使用 C11 原子内建，保证在 ISR/嵌套中不会丢失位 */
    __atomic_fetch_or(&s_tasks[tid].isr_event_flags, evt, __ATOMIC_RELAXED);
#else
    /* 如果编译器不支持原子内建，则仍进行简单 OR（在常见 Cortex-M 单核系统上通常可接受，但可能在嵌套 ISR 下有丢失风险） */
    s_tasks[tid].isr_event_flags |= evt;
#endif
}

/* 清除事件（主线程与 ISR 标志都清除） */
void SchedOpt_ClearEvent(tid_t tid, evt_t evt) {
    if (!valid_tid(tid)) return;
    __disable_irq();
    s_tasks[tid].event_flags &= ~evt;
    s_tasks[tid].isr_event_flags &= ~evt;
    __enable_irq();
}

/* StartDelayedEvent：先读取 wake（临界区外），再进入临界区写入并挂起 */
void SchedOpt_StartDelayedEvent(tid_t tid, evt_t evt, uint32_t delay_ms) {
    if (!valid_tid(tid)) return;
    uint32_t wake = SchedOpt_GetTick() + delay_ms;
    __disable_irq();
    s_task_t *t = &s_tasks[tid];
    t->delayed_evt |= evt;
    t->delay_until = wake;
    __enable_irq();
}

/* TaskDelay：挂起任务 delay_ms（不设置事件） */
void SchedOpt_TaskDelay(tid_t tid, uint32_t delay_ms) {
    if (!valid_tid(tid)) return;
    uint32_t wake = SchedOpt_GetTick() + delay_ms;	//取tick
    __disable_irq();
    s_task_t *t = &s_tasks[tid];
    t->delay_until = wake;
    if(t->in_queue){
    	/* 如果在队列中则移除 */
    	dequeue_task(tid);
    }
    t->in_queue = 0;
    t->delayed_evt = 0;	//清理历史延时事件，防止叠加
    // 保留 event_flags，不清除，避免延时期间丢事件
    t->state = TASK_STATE_WAITING;
    __enable_irq();
}

/* 将任务挂起（从就绪队列移除并设置为 SUSPENDED） */
void SchedOpt_SuspendTask(tid_t tid) {
    if (!valid_tid((int)tid)) return;
    __disable_irq();
    /* 如果在队列中则移除 */
    if (s_tasks[tid].in_queue) {
        dequeue_task((uint8_t)tid);
    }
    /* 清除就绪/事件状态（但保留结构以便以后恢复） */
    s_tasks[tid].state = TASK_STATE_SUSPENDED;
    s_tasks[tid].in_queue = 0;
    s_tasks[tid].next = NULL_TID;
    __enable_irq();
}

/* 恢复任务到 READY 并入队（如果已经在队列则什么也不做） */
void SchedOpt_ResumeTask(tid_t tid) {
    if (!valid_tid((int)tid)) return;
    __disable_irq();
    if (s_tasks[tid].state == TASK_STATE_SUSPENDED || s_tasks[tid].state == TASK_STATE_WAITING) {
    	s_tasks[tid].delay_until = 0;	 // 清除延时，避免二次唤醒
    	s_tasks[tid].state = TASK_STATE_READY;
        /* 仅当不在队列时入队 */
        if (!s_tasks[tid].in_queue) {
            enqueue_ready((uint8_t)tid);
        }
    }
    __enable_irq();
}

/* 删除任务：从队列移除并使其不再被调度（fn 置 NULL） */
void SchedOpt_DeleteTask(tid_t tid) {
    if (!valid_tid((int)tid)) return;
    __disable_irq();
    if (s_tasks[tid].in_queue) {
        dequeue_task((uint8_t)tid);
    }
    /* 清空事件和延时 */
    s_tasks[tid].event_flags = 0;
    s_tasks[tid].isr_event_flags = 0;
    s_tasks[tid].delayed_evt = 0;
    s_tasks[tid].delay_until = 0;

    /* 禁用任务 */
    s_tasks[tid].fn = NULL;
    s_tasks[tid].state = TASK_STATE_DELETED; /* 也可引入 TASK_STATE_DELETED */
    s_tasks[tid].in_queue = 0;
    s_tasks[tid].next = NULL_TID;

    __enable_irq();
}


/* RunOnce：合并 ISR、检查延时、选最高优先级任务并运行一次 */
void SchedOpt_RunOnce(void) {
    merge_isr_events();
    check_delayed_events();

    int prio = pick_highest_priority_index();
    if (prio < 0) return;

    __disable_irq();
    int tid = dequeue_ready_from_prio(prio);
    __enable_irq();

    if (tid < 0) return;
    if (!valid_tid(tid)) return;

    s_task_t *t = &s_tasks[tid];
    if (t->state != TASK_STATE_READY) return;
		
	#if SCHED_ENABLE_TASK_WATCHDOG
		uint32_t start = SchedOpt_GetTick();
	#endif
	
    t->state = TASK_STATE_RUNNING;

    /* 快照主事件位，清零（ISR 标志保留以供下轮合并） */
    __disable_irq();
    evt_t ev = t->event_flags;
    t->event_flags = 0;
    __enable_irq();

    if (t->fn) {
        t->fn((tid_t)tid, ev);
    }
		
	#if SCHED_ENABLE_TASK_WATCHDOG
		uint32_t end = SchedOpt_GetTick();
		if ((int32_t)(end - start) > (int32_t)SCHED_TASK_WATCHDOG_MS) {
			SchedOpt_ErrorHandler("task watchdog timeout");
		}
	#endif
		
    /* 若任务没有把自己置为 WAITING/SUSPENDED，则恢复 READY 并入队 */
    if (t->state == TASK_STATE_RUNNING) {
    	__disable_irq();
        t->state = TASK_STATE_READY;
        enqueue_ready((uint8_t)tid);
        __enable_irq();
    }
}

/* 永久运行：循环 RunOnce，并在无就绪时 WFI */
void SchedOpt_RunForever(void) {
    for (;;) {
        SchedOpt_RunOnce();
        if (s_ready_bitmap == 0) {
            __WFI();
        }
    }
}



